#include "stdafx.h"
#include "mainloop.h"
#include "draw.h"


extern CGlobal g_global;
extern CCamShiftHelper g_camshiftHelper;

extern HWND		g_hWnd;		// Holds Our Window Handle
extern HDC		g_compatibleDC;

void   mainLoop(void)
{
	ARUint8         *dataPtr;
	ARMarkerInfo    *marker_info;
	int             marker_num;
	int             j, k;

	/* grab a vide frame */
	if( (dataPtr = (ARUint8 *)arVideoGetImage()) == NULL ) {
		arUtilSleep(2);
		return;
	}

	// ----------------------
	// by aicro
	g_camshiftHelper._Fill_CV_IplImage(g_global.imgWidth, g_global.imgHeight, (char*)dataPtr);

	// put the image on the main window screen
	IplImage *image;
	g_camshiftHelper._GetImage(&image);
	// we copy the image to the global one that will be shown in the picture controller
	IplImage *temp = cvCreateImage(cvSize(g_global.imgWidth, g_global.imgHeight), IPL_DEPTH_8U, 3);
	cvCopy(image,  temp);	
	// we need vertically invert the image data
	cvConvertImage(temp, g_global.camCapturedImg, CV_CVTIMG_FLIP );
	cvReleaseImage(&temp);


	// call the camshift to get the position and angle of the 2Dbox
	g_camshiftHelper._ShowAdjustWindow(true);

	// here we take the center of the picture
	int x, y;
	g_camshiftHelper._GetDetectedCenter(&x, &y, g_global.windowZoom);
	// printf("x = %3d, y = %3d\n", x, y);

	// use opengl to get the inverse points in the world coordinates
	GLint viewport[4];
	glGetIntegerv(GL_VIEWPORT, viewport);
	
	GLdouble mvmatrix[16];
	glGetDoublev(GL_MODELVIEW_MATRIX, mvmatrix);
	
	GLdouble projmatrix[16];
	glGetDoublev(GL_PROJECTION_MATRIX, projmatrix);
	
	GLint realy = viewport[3] - (GLint)y;
	
	gluUnProject((GLdouble)x, (GLdouble)realy, 1.0,\
			      mvmatrix, projmatrix, viewport, \
				  &(g_global.wxDir), &(g_global.wyDir), &(g_global.wzDir) );
	
	gluUnProject((GLdouble)g_global.windowZoom*g_global.imgWidth/2, \
			    (GLdouble)g_global.windowZoom*g_global.imgHeight/2, \
				  0.0, mvmatrix, projmatrix, viewport, \
				  &(g_global.wxOri), &(g_global.wyOri), &(g_global.wzOri) );

	argDrawMode2D();
	argDispImage( dataPtr, 0,0 );



	/* detect the markers in the video frame */
	if( arDetectMarker(dataPtr, g_global.thresh, &marker_info, &marker_num) < 0 ) {
		cleanup();
		exit(0);
	}

	arVideoCapNext();

	/* check for object visibility */
	k = -1;
	for( j = 0; j < marker_num; j++ ) {
		if( g_global.patt_id == marker_info[j].id ) {
			if( k == -1 ) k = j;
			else if( marker_info[k].cf < marker_info[j].cf ) k = j;
		}
	}
	if( k == -1 ) {
		argSwapBuffers();
		// we do not get the visibility of the marker
		g_global.bDrawMarker = false;

		// notify the main thread that we have got the image data
		SetEvent(g_global.hGetImage);
		return;
	}
	else
	{
		// we get the visibility of the marker
		g_global.bDrawMarker = true;

		// notify the main thread that we have got the image data
		SetEvent(g_global.hGetImage);
	}

	/* get the transformation between the marker and the real camera */
	arGetTransMat(&marker_info[k], g_global.patt_center, g_global.patt_width, g_global.patt_trans);

	//printf("\n%f %f %f\n",patt_trans[0][3],patt_trans[1][3],patt_trans[2][3]);

	// those drawing is for AR's default windows
	draw(g_global.wxOri, g_global.wyOri, g_global.wzOri,\
		 g_global.wxDir, g_global.wyDir, g_global.wzDir);

	argSwapBuffers();

	// ----------------------
	// by aicro
	// test crash
	CCrashTest crashTest;
	// we should change the coordinates because of the size and location 
	// of the change of virtual character
	crashTest.calculateAABB(-25, 0, -25, 25, 50, 25);
	double t = crashTest.rayIntersect(g_global.wxOri, g_global.wyOri, g_global.wzOri,\
									  g_global.wxDir, g_global.wyDir, g_global.wzDir);

	if (t != -1)
	{
		// we have touch it
		g_global.bTouch = true;
		static int count = 0;
		printf("TOUCH!!	%d\n", count++);
	}
}